#include <iostream>
#include <vector>
#include <string>
#include <sstream>
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include "ArmourPlate.h"
#include "SerialPort.h"
#include "AttitudeSolution.h"


using namespace std;
using namespace cv;


int main(){
    ArmourPlate armo;                   //实例化一个装甲板类
    VideoCapture capture;               //打开摄像头
    //AttitudeSolution target_attitude;
    //SerialPort UART;                  //实例化串口
    //UART.SerialPort_init(UART);       //串口初始化
    capture.open("../2.avi");           //打开测试视频
    Mat frame;
    armo.kalman_init();                 //装甲板预测初始化
    int counts = 0;                     //循环总次数计数
    int suc_counts = 0;                 //成功识别次数计数
    vector<unsigned char> temptx;       //发送到stm32的数据
	while (1) {
                // if(UART.Open()==false)printf("OPEN error!\n");
	        // else printf("OPEN OK!\n");
                // if(UART.LoadConfig()==false)printf("Set error!\n");
	        // else printf("Set OK!\n");
                counts++;                                       //循环次数加1
                double n_time = (double)getTickCount();
	        capture >> frame;
		//frame = imread("../1.png");
                if(frame.empty()){                              //空检测
                         return -1;
                }
                Mat frame1 = frame.clone();                     //备份一份数据
		Mat binary = armo.Pretreatment(frame1);         //预处理
		imshow("binary", binary);
                vector<vector<Point>> selected_cotours = armo.Screen(binary);           //筛选轮廓
		vector<Rect> ttt = armo.matchingForArmo(selected_cotours);              //匹配轮廓
                                                                               
                if (ttt.size() != 0) {
                        suc_counts++;
                }else{
                        continue;                               //为空跳过，为了防止测距函数报错。
                }
		for (int i = 0; i < ttt.size(); i++) { 
                        //cout << ttt.size() << endl;
			rectangle(frame, ttt[i], Scalar(0, 255, 0), 2, 8);              //画框
		}
                //if(!ttt.empty()){
                //target_attitude.Similarity(ttt[0]);
                //cout <<"目标距离：" << target_attitude.distance_similarity <<endl;
                //}
                //armo.amou_center = Point(ttt[0].tl().x+ttt[0].width/2,ttt[0].tl().y+ttt[0].height/2);
                //Point predictP = armo.kalman_predict(armo.amou_center);
                //circle(frame,predictP,100,Scalar(0,255,0),2,8);
                //cout << " 成功率：" << (double)suc_counts / counts <<endl;
		imshow("hua", frame);                             
                //temptx = UART.SendMessage(26,9);
                //UART<<temptx;
                waitKey(25);

                n_time = ((double)getTickCount()-n_time)/getTickFrequency();
                //cout<<"          "<<target_attitude.yaw_angle_similarity<<"          "<<target_attitude.pitch_angle_similarity<<endl;
                cout << " 用时：" << n_time <<endl;

		
	}
        //UART.Close();
         return 0;
} 

